﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Drawing;
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using W3C.Soap;
using Microsoft.Robotics.PhysicalModel;

namespace JaguarControl
{
    /// <summary>
    /// JaguarControl state
    /// </summary>
    [DataContract]
    public class JaguarControlState
    {
        private Robot _Jaguar = new Robot();
        [DataMember]
        public Robot Jaguar
        {
            get { return _Jaguar; }
            set { _Jaguar = value; }
        }
    }

    [DataContract]
    public class Robot
    {
        #region COORDINATION
        private Coordination _Coordination = new Coordination();
        [DataMember]
        public Coordination Coordination
        {
            get { return _Coordination; }
            set { _Coordination = value; }
        }
        #endregion

        #region SENSORS
        private Sensors _Sensors = new Sensors();
        [DataMember]
        public Sensors Sensors
        {
            get { return _Sensors; }
            set { _Sensors = value; }
        }
        #endregion

        #region POSE
        private Physics _Physics = new Physics();
        [DataMember]
        public Physics Pose
        {
            get { return _Physics; }
            set { _Physics = value; }
        }
        #endregion

        #region SLAM and VFH
        private Map map = new Map();

        [DataMember]
        [Description("The objects related to simultaneous localization and mapping")]
        public Map MAP
        {
            get { return map; }
            set { map = value; }
        }

        private VFH vfh = new VFH();

        [DataMember]
        [Description("The objects related to VFH")]
        public VFH VFH
        {
            get { return vfh; }
            set { vfh = value; }
        }

        #endregion
    }

    #region Robot Coordination Classes
    /// <summary>
    /// LIST OF THE ELIGIBLE TASKS
    /// This can be useful for the FANOUT metric, substracting 'score' each time unknown is active.
    /// </summary>
    [DataContract]
    public enum Task
    {
        Unknown,
        Explore_Map,
        Recognize_Identify,
        Support_Relief,
    }

    /// <summary>
    /// LIST OF THE TASKS' STATUS
    /// This can be useful for the metrics TASK TIME DEVELOPMENT in collaboration with Data Member TaskCountdown
    /// </summary>
    [DataContract]
    public enum TaskStatus
    {
        Unknown,
        Halted,
        InProcess,
        NeedHelp,

    }

    /// <summary>
    /// LIST OF THE ELIGIBLE GLOBAL STATUS
    /// </summary>
    [DataContract]
    public enum GlobalStatus
    {
        Unknown,
        Busy,
        Idle,
        Helping
    }

    /// <summary>
    /// LIST OF THE ELIGIBLE ROLES
    /// </summary>
    [DataContract]
    public enum Role
    {
        Unknown,
        FireBrigade,            //For damage control, relief and support
        AmbulanceTeam,          //For civilian transport, relief and support
        PoliceForce,            //Clearing blockades, mapping, exploring
        Rescuer,                //Aid kin by pushing or going towards,
        Trapped,                //Asking help, Malfunction, Notifying a physical problem.
    }

    /// <summary>
    /// LIST OF THE ELIGIBLE BEHAVIORS
    /// </summary>
    [DataContract]
    public enum ActiveBehavior
    {
        WakeUp,
        Resume,
        Wait,
        HandleCollision,
        AvoidPast,
        Locate,
        DriveTowards,
        SafeWander,
        Seek,
        PathPlanning,
        Aggregate,
        UCLine,
        UCColumn,
        UCDiamond,
        UCWedge,
        HoldFormation,
        Lost,
        Flock,
        Disperse,
        FieldCover,
        WallFollow,
        Escape,
        Report,
        Track,
        Inspect,
        Victim,
        Threat,
        Kin,
        GiveAid,
        Aid_Transport,
        Aid_DisplayInfo,
        Aid_ExtinguishFire,
        Aid_AlertHumans,
        Impatient,
        Acquiescent,
        Unknown
    }

    /// <summary>
    /// LIST OF STATES FOR THE FSM
    /// This must match the sequence diagrams of each operation
    /// </summary>
    [DataContract]
    public enum OperationsState
    {
        Idle,
        Wandering,
        Aggregated,
        Forming,
        FlockingReady,
        Dispersed,
        FieldCoverReady,
        WallFollowing
    }

    [DataContract]
    public class Flags
    {
        [DataMember]
        public bool WakeUpRequest = false;
        [DataMember]
        public bool ResumeRequest = false;
        [DataMember]
        public bool WaitRequest = false;
        [DataMember]
        public bool HandleCollisionRequest = false;
        [DataMember]
        public bool WaypointsOverload = false;
        [DataMember]
        public bool LocateRequest = false;
        [DataMember]
        public bool DriveTowardsRequest = false;
        [DataMember]
        public bool SafeWanderRequest = false;
        [DataMember]
        public bool SeekRequest = false;
        [DataMember]
        public bool PathPlanRequest = false;
        [DataMember]
        public bool EnoughMap = false;
        [DataMember]
        public bool AggregateRequest = false;
        [DataMember]
        public bool LineFormRequest = false;
        [DataMember]
        public bool ColumnFormRequest = false;
        [DataMember]
        public bool DiamondFormRequest = false;
        [DataMember]
        public bool WedgeFormRequest = false;
        [DataMember]
        public bool HoldFormationRequest = false;
        [DataMember]
        public bool LostRequest = false;
        [DataMember]
        public bool FlockRequest = false;
        [DataMember]
        public bool FlockIsReady = false;
        [DataMember]
        public bool DisperseRequest = false;
        [DataMember]
        public bool FieldCoverRequest = false;
        [DataMember]
        public bool FieldCoverIsReady = false;
        [DataMember]
        public bool WallFollowRequest = false;
        [DataMember]
        public bool InsideRoom = false;
        [DataMember]
        public bool InLocalMinima = false;
        [DataMember]
        public bool EscapeRequest = false;
        [DataMember]
        public bool ReportRequest = false;
        [DataMember]
        public bool TrackRequest = false;
        [DataMember]
        public bool InspectRequest = false;
        [DataMember]
        public bool VictimDetected = false;
        [DataMember]
        public bool ThreatDetected = false;
        [DataMember]
        public bool KinHelpRequest = false;
        [DataMember]
        public bool GiveAidRequest = false;
        [DataMember]
        public bool AidTransportRequest = false;
        [DataMember]
        public bool AidDisplayInfoRequest = false;
        [DataMember]
        public bool AidExtinguishFireRequest = false;
        [DataMember]
        public bool AidAlertHumansRequest = false;
        [DataMember]
        public bool AcquiescentRequest = false;
        [DataMember]
        public bool SysError = false;

    }

    [DataContract]
    public class Waypoints
    {
        [DataMember]
        public DateTime LastTime = new DateTime();

        [DataMember]
        public List<Points> Points = new List<Points>();

        [DataMember]
        public Tuple<bool, int> LastPoint = new Tuple<bool, int>();
    }

    [DataContract]
    public class Points
    {
        [DataMember]
        public int X;

        [DataMember]
        public int Z;

        [DataMember]
        public int Steps;
    }
        
    [DataContract]
    public class Coordination
    {
        #region High-level Coordination
        Task _Task = Task.Unknown;
        TaskStatus _TaskStatus = TaskStatus.Unknown;
        GlobalStatus _GlobalStatus = GlobalStatus.Unknown;
        Role _Role = Role.Unknown;
        private bool _HighLvl_Command = false;
        private int _ID;
        
        [DataMember]
        public Vector2 ID2Pose = new Vector2();

        [DataMember]
        [Description("Contains enum value for the current active task")]
        public Task Task
        {
            get { return _Task; }
            set { _Task = value; }
        }

        [DataMember]
        [Description("Contains enum value for the current active task's status")]
        public TaskStatus TaskStatus
        {
            get { return _TaskStatus; }
            set { _TaskStatus = value; }
        }

        [DataMember]
        [Description("Contains enum value for the current global/overall robot status")]
        public GlobalStatus Status
        {
            get { return _GlobalStatus; }
            set { _GlobalStatus = value; }
        }

        [DataMember]
        [Description("Contains enum value for the current active role")]
        public Role Role
        {
            get { return _Role; }
            set { _Role = value; }
        }

        [DataMember]
        [Description("For System element communications in a JAUS-like architecture")]
        public bool OCU_Override
        {
            get { return _HighLvl_Command; }
            set { _HighLvl_Command = value; }
        }

        [DataMember]
        [Description("For identyfying the robot from robot peers")]
        public int ID
        {
            get { return _ID; }
            set { _ID = value; }
        }

        public void ResetStatus()
        {
            Role = JaguarControl.Role.PoliceForce;
            Behavior = ActiveBehavior.Resume;
            Task = JaguarControl.Task.Explore_Map;
            TaskStatus = TaskStatus.InProcess;
            Status = GlobalStatus.Idle;
            FSMstate = OperationsState.Idle;
            OCU_Override = false;
           
        }

        #endregion

        #region Low-level Coordination
        ActiveBehavior _Behavior = ActiveBehavior.Resume;
        OperationsState _FSMstate = OperationsState.Idle;
        private int _Behavior_Count = 0;
        private int _Task_Count = 0;
        private bool _DriveInProcess = false;
        private bool _InGoal = false;
        private bool _IsDocked = false;

        [DataMember]
        [Description("Contains enum value for the current active robot behavior")]
        public ActiveBehavior Behavior
        {
            get { return _Behavior; }
            set { _Behavior = value; }
        }

        [DataMember]
        [Description("Contains enum value for the current active robot fsm state")]
        public OperationsState FSMstate
        {
            get { return _FSMstate; }
            set { _FSMstate = value; }
        }

        [DataMember]
        [Description("For measuring the constant steps using the same active behavior")]
        public int BehaviorCount
        {
            get { return _Behavior_Count; }
            set { _Behavior_Count = value; }
        }

        [DataMember]
        [Description("For measuring the constant steps using the same active task")]
        public int TaskCount
        {
            get { return _Task_Count; }
            set { _Task_Count = value; }
        }

        [DataMember]
        [Description("For determining if the drive is pending a turning or translating operation")]
        public bool DriveInProcess
        {
            get { return _DriveInProcess; }
            set { _DriveInProcess = value; }
        }

        [DataMember]
        [Description("For determining if going towards a location has been achieved")]
        public bool InGoal
        {
            get { return _InGoal; }
            set { _InGoal = value; }
        }

        [DataMember]
        [Description("For determining if the robot is in formation required position")]
        public bool IsDocked
        {
            get { return _IsDocked; }
            set { _IsDocked = value; }
        }

        public void ResetVars()
        {
            BehaviorCount = 0;
            TaskCount = 0;
            DriveInProcess = false;
            InGoal = false;
            IsDocked = false;
        }
        #endregion
    }
    #endregion

    #region Robot Pose Classes
    [DataContract]
    public class Physics
    {
        [DataMember]
        public double Speed;
        [DataMember]
        public double LeftSpeed;
        [DataMember]
        public double RightSpeed;
        [DataMember]
        public double Heading;
        [DataMember]
        public double X_pose;
        [DataMember]
        public double Y_pose;
        [DataMember]
        public double Z_pose;  
        //[DataMember]
        //public Vector3 VelocityVector;
        [DataMember]
        public Vector3 OrientationVector = new Vector3(); //R,P,Y
        [DataMember]
        public Vector3 Target = new Vector3(); //X,Y,Theta

        private Waypoints _waypoints = new Waypoints();
        [DataMember]
        [Description("For determining the traversed path and having a count of the times passed over it")]
        public Waypoints Waypoints
        {
            get { return _waypoints; }
            set { _waypoints = value; }
        }

    }
    #endregion 

    #region Robot Sensors Classes
    [DataContract]
    public class Sensors
    {
        //For allocating the Frame Image RGB Raw Data, Image Format, Size, and TimeStamp
        [DataMember]
        public Image CameraData;

        [DataMemberConstructor]
        public MainForm.EncoderRecord EncodersData;

        [DataMemberConstructor]
        public MainForm.IMURecord IMUData;

        [DataMemberConstructor]
        public MainForm.GPSRecord GPSData;

        [DataMember]
        public int[] LaserData;

        [DataMember]
        public bool StuckDetect;

        [DataMember]
        public bool TempDetect;

        [DataMember]
        public double BatteryVoltage;

        [DataMemberConstructor]
        public FromWinformMsg.BatteryEnum BatteryState;

    }
    #endregion

    #region SLAM and VFH Classes
    [DataContract]
    public enum MapMode
    {
        Overwrite,      // Just write new data (free or occupied) into the map
        Counting       // Count the number of times a cell is seen free (or occ)
        //BayesRule       // Use Bayes Rule to update cell values
    }

    [DataContract]
    public class Map
    {
        [DataMember]
        public double MapWidth = 20;
        [DataMember]
        public double MapHeight = 20;
        [DataMember]
        public double MapResolution = 0.15;
        [DataMember]
        public double MapMaxRange = 7.99;
        [DataMember]
        public bool DrawMap = true;
        [DataMember]
        public MapMode DrawingMode = MapMode.Counting;
    }

    [DataContract]
    public class VFH
    {
        [DataMember]
        public bool DrawVFH = true;
        [DataMember]
        public int Thresh = 1000; //mm Same as AwareofObstacle distance.
        [DataMember]
        public int Min4Candid = 100; //Consecutive measurements to form a candidate.
    }
    #endregion

}
